﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_SlamPossitioning.cs
 * \brief	This file contains the SlamPossitioning functions function that 
 *  
 * This file contains the ObjectIdentification function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodríguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2009-12-16
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                       2009-12-16 \n
 *      First implementation of the M44_SlamPossitioning class.
 * 
 * - Version 0.02:   Gonzalo RODRIGUEZ                       2010-02-16 \n
 *      Separate some functions in other files to simplify the understanding as well as commenting all the functions present.
 * - Version 0.03    Gonzalo RODRIGUEZ                      2010-02-17  \n
 *      Changes in slam functions. input changed from cl_RobotStateEstimate to cl_RoborGPSINSEstimate 
 *
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_SlamPossitioning
     * 
     *  \brief  This is the class of the M44_SlamPossitioning functions.
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_SlamPossitioning
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static r_SlamResults Slam( float[][] Lrf3DMeasure,
                                                  cl_SensorData Sensors,
                                                  cl_Map2D HighResMap,
                                                  cl_LowResMap LowResMapGlobal,
                                                  float max_error_position,
                                                  cl_RobotStateEstimate r_RobotStateEstimate,
                                                  float f_tau_free, float f_tau_confidence)
         * 
         *  \brief This function finds the minimum position and the correspondent error for  
         *         the cell-slam and the subcell-slam.
         *  
         *  \param[in] float[][] Lrf3DMeasure,
         *  \param[in] cl_SensorData Sensors,
         *  \param[in] cl_Map2D HighResMap,
         *  \param[in] cl_LowResMap LowResMapGlobal,
         *  \param[in] float max_error_position,
         *  \param[in] cl_RobotStateEstimate r_RobotStateEstimate,
         *  \param[in] float f_tau_free, 
         *  \param[in] float f_tau_confidence
         */
        /* ------------------------------------------------------------------------- */

        public static cl_Pos3D Slam(cl_3DLrfMeasure LaserMeasure,
                                                            cl_Orientation3D Orientation,
                                                            cl_SensorData Sensors,
                                                            cl_Map2D HighResMap,
                                                            cl_Map2D LowResMapGlobal,
                                                            float max_error_position,
                                                            cl_RobotGPSINSState r_RobotStateEstimate)
        {
            bool b_continue_search = true;
            int i_n_search = 0;
            double[] td_epsilon = new double[3];//epsilon x y and z (offset)
            double[] td_PosMin = new double[2];
            double d_eMin = 10000;
            int[] ti_RobotCell = new int[2];
            int[] ti_MinIndex = new int[2];
            double[] td_RobotPos = new double[2];
            double[ , ] td_eGrid = new double[3, 3] , td_eGrid2 = new double[3, 3], td_eGrad = new double[3, 3];
            double[ , , ] td_PosGrid = new double[3, 3, 2] , td_PosGrid2 = new double[3, 3, 2];
            int i_ig, i_jg;
            double[,] ti_AddVector = new double[3, 2];
            double d_Scale = (double)LowResMapGlobal.us_Scale;
            double[,] td_SubCellPoints = new double[9, 2];
            double d_MaxEGrid;
            double[] td_MaxEGridPos = new double[2];
            float[] tf_move_in_grid = new float[2];
            float[] tf_RoundingError;
            float f_maxMeasuredHeights = 0;
            double[] td_aux_Pos = new double[2];
            float[] tf_aux_Pos = new float[2];
            float[,] Lrf3DMeasure = LaserMeasure.tf_3DLrfDistance;
            M44_auxSlamStr.cl_HRMResults HRM_results;
            M44_auxSlamStr.cl_SlamErrorsResults r_SlamErrorsResults;
            M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam;
            M44_auxSlamStr.cl_SlamTest r_SlamTest;
            M44_auxSlamStr.cl_SlamTestErrors r_SlamTestErrors;
            M44_auxSlamStr.cl_SubCellErrors r_SubCellErrors;
            M44_auxSlamStr.cl_NewGrids r_NewGrids;
            M44_auxSlamStr.cl_SlamResults r_SlamResults = new M44_auxSlamStr.cl_SlamResults(2);

            //Local Map on the Robot Reference
            td_epsilon[0] = 1;
            td_epsilon[1] = 1;
            td_epsilon[2] = 1;

            /*Output data*/
            cl_Pos3D tf_SlamOutput = new cl_Pos3D();

            ti_RobotCell = M44_BasicClass.Coordinates2Cell(LowResMapGlobal, r_RobotStateEstimate.RobotEstimate.td_Pos);

            HRM_results = HRM_Creation_Slam(LaserMeasure,Orientation, Sensors, HighResMap, LowResMapGlobal, r_RobotStateEstimate, ti_RobotCell, td_epsilon);
            aux_MapCell_local_Slam = HRM_results.aux_MapCell_local_slam;
            tf_RoundingError = HRM_results.RoundingError;

            for(int i=0;i<HighResMap.us_Nrows;i++)
            {
                for(int j=0;j<HighResMap.us_Ncols;j++)
                {
                    if (Math.Abs(aux_MapCell_local_Slam.Measured_Heights[i,j])>f_maxMeasuredHeights)
                        f_maxMeasuredHeights = aux_MapCell_local_Slam.Measured_Heights[i,j];
                }
            }

            if (f_maxMeasuredHeights < 0.01)
            {
                r_SlamResults.d_eSlam = -1;
                r_SlamResults.tf_PosSlam = r_RobotStateEstimate.RobotEstimate.td_Pos;
                r_SlamResults.d_eSubSlam = -1;
                r_SlamResults.tf_PosSubSlam = r_RobotStateEstimate.RobotEstimate.td_Pos;
                
            }
            else
            {
                //slam errors calculation
                
                
                r_SlamErrorsResults = M44_SlamErrors.SlamErrors(ti_RobotCell, aux_MapCell_local_Slam, tf_RoundingError,max_error_position,LowResMapGlobal,HighResMap);
                r_SlamTest = r_SlamErrorsResults.SlamTest;
                r_SlamTestErrors = r_SlamErrorsResults.SlamTestErrors;

                //Save results in output structure
                r_SlamResults.d_eSlam = r_SlamTest.Global_total_error.Error_value;
                r_SlamResults.tf_PosSlam = r_SlamTest.Global_total_error.robotPositionEstimate;

                //SubCellError calculation

                ti_RobotCell[0] = r_SlamTest.Global_total_error.robotCellEstimate[0];
                ti_RobotCell[1] = r_SlamTest.Global_total_error.robotCellEstimate[1];

                tf_aux_Pos = M44_BasicClass.Cell2Coordinates(LowResMapGlobal, ti_RobotCell);
                td_RobotPos[0] = (double)tf_aux_Pos[0];
                td_RobotPos[1] = (double)tf_aux_Pos[1];

                td_SubCellPoints[0, 0] = td_RobotPos[0] + d_Scale / 4;
                td_SubCellPoints[0, 1] = td_RobotPos[1] + d_Scale / 4;

                td_SubCellPoints[1, 0] = td_RobotPos[0] + d_Scale / 2;
                td_SubCellPoints[1, 1] = td_RobotPos[1] + d_Scale / 4;

                td_SubCellPoints[2, 0] = td_RobotPos[0] + d_Scale / (4 / 3);
                td_SubCellPoints[2, 1] = td_RobotPos[1] + d_Scale / 4;

                td_SubCellPoints[3, 0] = td_RobotPos[0] + d_Scale / 4;
                td_SubCellPoints[3, 1] = td_RobotPos[1] + d_Scale / 2;

                td_SubCellPoints[4, 0] = td_RobotPos[0] + d_Scale / 2;
                td_SubCellPoints[4, 1] = td_RobotPos[1] + d_Scale / 2;

                td_SubCellPoints[5, 0] = td_RobotPos[0] + d_Scale / (4 / 3);
                td_SubCellPoints[5, 1] = td_RobotPos[1] + d_Scale / 2;

                td_SubCellPoints[6, 0] = td_RobotPos[0] + d_Scale / 4;
                td_SubCellPoints[6, 1] = td_RobotPos[1] + d_Scale / (4 / 3);

                td_SubCellPoints[7, 0] = td_RobotPos[0] + d_Scale / 2;
                td_SubCellPoints[7, 1] = td_RobotPos[1] + d_Scale / (4 / 3);

                td_SubCellPoints[8, 0] = td_RobotPos[0] + d_Scale / (4 / 3);
                td_SubCellPoints[8, 1] = td_RobotPos[1] + d_Scale / (4 / 3);

                i_ig = 0;
                i_jg = 0;
                for (int k = 0; k < 9; k++)
                {
                    td_epsilon[0] = td_SubCellPoints[k,0] - r_RobotStateEstimate.RobotEstimate.td_Pos[0];
                    td_epsilon[1] = td_SubCellPoints[k,1] - r_RobotStateEstimate.RobotEstimate.td_Pos[2];
                    td_epsilon[2] = 0;
                    double[] td_Pos_aux = new double[2];
                    td_Pos_aux[0] = r_RobotStateEstimate.RobotEstimate.td_Pos[0]+td_epsilon[0];
                    td_Pos_aux[1] = r_RobotStateEstimate.RobotEstimate.td_Pos[2]+td_epsilon[1];
                    ti_RobotCell = M44_BasicClass.Coordinates2Cell(LowResMapGlobal, td_Pos_aux );
                    HRM_results = HRM_Creation_Slam(LaserMeasure,Orientation, Sensors, HighResMap, LowResMapGlobal, r_RobotStateEstimate, ti_RobotCell, td_epsilon);
                    aux_MapCell_local_Slam = HRM_results.aux_MapCell_local_slam;
                    tf_RoundingError = HRM_results.RoundingError;

                    r_SubCellErrors = M44_OnePointErrors.OnePointErrors(aux_MapCell_local_Slam, ti_RobotCell, r_SlamTestErrors.Sum_errors, LowResMapGlobal, HighResMap);
                    
                    if (r_SubCellErrors.eTotalGlobal < d_eMin || k == 1)
                    {
                        d_eMin = r_SubCellErrors.eTotalGlobal;
                        td_PosMin[0] = td_SubCellPoints[k,0];
                        td_PosMin[1] = td_SubCellPoints[k,1];
                        ti_MinIndex[0] = i_ig;
                        ti_MinIndex[1] = i_jg;
                    }
                    td_eGrid[i_ig, i_jg] = r_SubCellErrors.eTotalGlobal;       
                    td_eGrad[i_ig, i_jg] = r_SubCellErrors.eTotalGrad;
                    td_PosGrid[i_ig,i_jg,0] = td_SubCellPoints[k,0];
                    td_PosGrid[i_ig,i_jg,1] = td_SubCellPoints[k,1];
                    i_jg++;
                    if (i_jg == 3)
                    {
                        i_ig++;
                        i_jg = 0;
                    }
                }
                //Search for the minimum
                while (b_continue_search && (i_n_search < 2))
                {
                    if (ti_MinIndex[0] == 1 && ti_MinIndex[1] == 1)
                    {
                        b_continue_search = false;
                        break;
                    }
                    if (ti_MinIndex[1] == 0 || ti_MinIndex[1] == 2)
                    {
                        if (ti_MinIndex[0] == 0)
                        {
                            ti_AddVector[0,0] = -d_Scale / 2;
                            ti_AddVector[0,1] = 0;
                            ti_AddVector[1,0] = -d_Scale / 2;
                            ti_AddVector[1,1] = -d_Scale / 2;
                            ti_AddVector[2,0] = 0;
                            ti_AddVector[2,1] = -d_Scale / 2;
                        }
                        else if (ti_MinIndex[0] == 1)
                        {
                            ti_AddVector[0,0] = -d_Scale / 2;
                            ti_AddVector[0,1] = 0;
                            ti_AddVector[1,0] = -d_Scale / 2;
                            ti_AddVector[1,1] = -d_Scale / 2;
                            ti_AddVector[2,0] = -d_Scale / 2;
                            ti_AddVector[2,1] = d_Scale / 2;
                        }
                        else if (ti_MinIndex[0] == 2)
                        {
                            ti_AddVector[0,0] = -d_Scale / 2;
                            ti_AddVector[0,1] = 0;
                            ti_AddVector[1,0] = -d_Scale / 2;
                            ti_AddVector[1,1] = d_Scale / 2;
                            ti_AddVector[2,0] = 0;
                            ti_AddVector[2,1] = d_Scale / 2;
                        }
                        if (ti_MinIndex[1] == 2)
                        {
                            ti_AddVector[0,0] = -ti_AddVector[0,0];
                            ti_AddVector[1,0] = -ti_AddVector[1,0];
                            ti_AddVector[2,0] = -ti_AddVector[2,0];
                        }
                    }
                    if (ti_MinIndex[1] == 1 && ti_MinIndex[0] == 0)
                    {
                        ti_AddVector[0,0] = -d_Scale / 2;
                        ti_AddVector[0,1] = -d_Scale / 2;
                        ti_AddVector[1,0] = 0;
                        ti_AddVector[1,1] = -d_Scale / 2;
                        ti_AddVector[2,0] = d_Scale / 2;
                        ti_AddVector[2,1] = -d_Scale / 2;
                    }
                    else if (ti_MinIndex[1] == 1 && ti_MinIndex[0] == 2)
                    {
                        ti_AddVector[0,0] = -d_Scale / 2;
                        ti_AddVector[0,1] = d_Scale / 2;
                        ti_AddVector[1,0] = 0;
                        ti_AddVector[1,1] = d_Scale / 2;
                        ti_AddVector[2,0] = d_Scale / 2;
                        ti_AddVector[2,1] = d_Scale / 2;
                    }
                    r_NewGrids = NewEGridCalculation(td_eGrid,td_PosGrid,ti_MinIndex);
                    td_eGrid2 = r_NewGrids.eGrid;
                    td_PosGrid2 = r_NewGrids.PosGrid;

                    //Calculate the maximum for e_grid2
                    d_MaxEGrid = td_eGrid2[1,1];
                    td_MaxEGridPos[0] = td_PosGrid2[1,1,0];
                    td_MaxEGridPos[1] = td_PosGrid2[1,1,1];

                    for (int i3 = 0; i3 < 3; i3++)
                    {
                        for (int j3 = 0; j3 < 3; j3++)
                        {
                            if (td_eGrid2[i3,j3] > d_MaxEGrid)
                            {

                                d_MaxEGrid = td_eGrid2[i3,j3];
                                td_MaxEGridPos[0] = td_PosGrid2[i3,j3,0];
                                td_MaxEGridPos[1] = td_PosGrid2[i3,j3,1];
                            }
                            if (td_eGrid2[i3,j3] == 0)
                                td_eGrid2[i3,j3] = (float)Math.Ceiling((double)d_MaxEGrid);
                        }
                    }
                    for (int k = 0; k < 3; k++)
                    {
                        td_epsilon[0] = td_PosMin[0] + ti_AddVector[k,0] - r_RobotStateEstimate.RobotEstimate.td_Pos[0];
                        td_epsilon[1] = td_PosMin[1] + ti_AddVector[k,1] - r_RobotStateEstimate.RobotEstimate.td_Pos[2];
                        td_epsilon[2] = 0;
                        td_aux_Pos[0] = r_RobotStateEstimate.RobotEstimate.td_Pos[0] + td_epsilon[0];
                        td_aux_Pos[1] = r_RobotStateEstimate.RobotEstimate.td_Pos[2] + td_epsilon[1];
                        ti_RobotCell = M44_BasicClass.Coordinates2Cell(LowResMapGlobal,td_aux_Pos);
                        if (ti_RobotCell[0] < 0 || ti_RobotCell[0] > LowResMapGlobal.us_Nrows ||
                            ti_RobotCell[1] < 0 || ti_RobotCell[1] > LowResMapGlobal.us_Ncols)
                        {
                            td_eGrid2[1 + (int)tf_move_in_grid[0], 1 + (int)tf_move_in_grid[1]] = -1;
                        }
                        else
                        {
                            HRM_results = HRM_Creation_Slam(LaserMeasure,Orientation, Sensors, HighResMap, LowResMapGlobal, r_RobotStateEstimate, ti_RobotCell, td_epsilon);
                            r_SubCellErrors = M44_OnePointErrors.OnePointErrors(aux_MapCell_local_Slam, ti_RobotCell, r_SlamTestErrors.Sum_errors, LowResMapGlobal, HighResMap);
                            td_eGrid2[1 + (int)tf_move_in_grid[0],1 + (int)tf_move_in_grid[1]] = r_SubCellErrors.eTotalGlobal;
                        
                        }
                        tf_move_in_grid[0] = (float)(ti_AddVector[k, 0] / (d_Scale / 2));
                        tf_move_in_grid[1] = (float)(ti_AddVector[k, 1] / (d_Scale / 2));

                        td_PosGrid2[1 + (int)tf_move_in_grid[0], 1 + (int)tf_move_in_grid[1], 0] = td_PosMin[0] + ti_AddVector[k, 0];
                        td_PosGrid2[1 + (int)tf_move_in_grid[0], 1 + (int)tf_move_in_grid[1], 1] = td_PosMin[1] + ti_AddVector[k, 1];

                    }
                    d_eMin = td_eGrid2[0,0];
                    td_PosMin[0] = td_PosGrid2[0,0,0];
                    td_PosMin[1] = td_PosGrid2[0,0,1];
                    for (int i3 = 0; i3 < 3; i3++)
                    {
                        for (int j3 = 0; j3 < 3; j3++)
                        {
                            if (td_eGrid2[i3,j3] < d_eMin)
                            {
                                d_eMin = td_eGrid2[i3,j3];
                                td_PosMin[0] = td_PosGrid2[i3,j3,0];
                                td_PosMin[1] = td_PosGrid2[i3,j3,1];
                            }
                        }
                    }
                    //Save results to output structure
                    r_SlamResults.d_eSubSlam = d_eMin;
                    r_SlamResults.tf_PosSubSlam = td_PosMin;

                    td_eGrid = td_eGrid2;
                    td_PosGrid = td_PosGrid2;
                    i_n_search++;
                    
                }
            }
            if (r_SlamResults.tf_PosSubSlam[0] == 0.0f && r_SlamResults.tf_PosSubSlam[1] == 0.0f)
            {
                r_SlamResults.d_eSubSlam = -1;
            }

            if (r_SlamResults.d_eSlam < r_SlamResults.d_eSubSlam && r_SlamResults.d_eSlam > 0)
            {
                tf_SlamOutput.f_x = (float)r_SlamResults.tf_PosSlam[0];
                tf_SlamOutput.f_y = (float)r_RobotStateEstimate.RobotEstimate.td_Pos[1];
                tf_SlamOutput.f_z = (float)r_SlamResults.tf_PosSlam[1];
            }
            else if (r_SlamResults.d_eSubSlam > 0)
            {
                tf_SlamOutput.f_x = (float)r_SlamResults.tf_PosSubSlam[0];
                tf_SlamOutput.f_y = (float)r_RobotStateEstimate.RobotEstimate.td_Pos[1];
                tf_SlamOutput.f_z = (float)r_SlamResults.tf_PosSubSlam[1];
            }
            
            /*The error is computed and returned*/
            tf_SlamOutput.f_x = (float)(tf_SlamOutput.f_x - r_RobotStateEstimate.RobotEstimate.td_Pos[0]);
            tf_SlamOutput.f_y = (float)(tf_SlamOutput.f_y - r_RobotStateEstimate.RobotEstimate.td_Pos[1]);
            tf_SlamOutput.f_z = (float)(tf_SlamOutput.f_z - r_RobotStateEstimate.RobotEstimate.td_Pos[2]);

            return tf_SlamOutput;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public static M44_auxSlamStr.cl_HRMResults HRM_Creation_Slam(cl_3DLrfMeasure LaserMeasure,
                                                        cl_SensorData Sensors,
                                                        cl_Map2D HighResMap,
                                                        cl_Map2D LowResMapGlobal,
                                                        cl_RobotStateEstimate r_RobotStateEstimate,
                                                        int[] ti_RobotCell, 
                                                        double[] td_epsilon)
         * 
         *  \brief This function creates a High Resolution Map 
         *  
         *  \param[in] float[][] Lrf3DMeasure,
         *  \param[in] cl_SensorData Sensors,
         *  \param[in] cl_Map2D HighResMap,
         *  \param[in] cl_LowResMap LowResMapGlobal,
         *  \param[in] cl_RobotStateEstimate r_RobotStateEstimate,
         *  \param[in] int[] ti_RobotCell,
         *  \param[in] double[] td_epsilon, 
         *  
         *  \param[out] M44_auxSlamStr.cl_HRMResults HRM_results
         */
        /* ------------------------------------------------------------------------- */

        public static M44_auxSlamStr.cl_HRMResults HRM_Creation_Slam(cl_3DLrfMeasure LaserMeasure,
                                                        cl_Orientation3D Orientation,
                                                        cl_SensorData Sensors,
                                                        cl_Map2D HighResMap,
                                                        cl_Map2D LowResMapGlobal,
                                                        cl_RobotGPSINSState r_RobotStateEstimate,
                                                        int[] ti_RobotCell, 
                                                        double[] td_epsilon)
        {
            ArrayList[,] CellList = null;

            float[] tf_RobotPosOrigin = new float[3];
            float f_RobotPosOriginHeight;
            double[] td_RoundingError = new double[3];
            float f_Distance;
           // float[] tf_CartesianLocalCoord = new float[3];
            double[,] tf_CartesianLocalCoord;
            float[] tf_3DLRFHorAngularRange, tf_3DLRFVerAngularRange;
            float[] tf_3DLRFHorAngularResolution,tf_3DLRFVerAngularResolution;
            float f_3DLRFRange;
            float f_alphaMin , f_sigmaMax;
            //float[] tf_CollisionPoint = new float[3];
            double[] tf_CollisionPoint;
            int colcounter = 0;
            int[] ti_AssignedCell = new int[2];
            double[,] td_aux_Cartesian = new double[3,1];
            double[] td_aux = new double[3];
            float[,] Lrf3DMeasure = LaserMeasure.tf_3DLrfDistance;
            int index_i, index_j;
            aux_MapCell aux_MapCell_local_Slam = new aux_MapCell(HighResMap.us_Nrows,HighResMap.us_Ncols);
            M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam2 = new M44_auxSlamStr.cl_auxMapCell(HighResMap.us_Nrows,HighResMap.us_Ncols);
            M44_auxSlamStr.cl_HRMResults HRM_results = new M44_auxSlamStr.cl_HRMResults(LowResMapGlobal.us_Nrows,LowResMapGlobal.us_Ncols);

            double debug_aux1;
            //Sensor Data
            tf_3DLRFHorAngularRange         = Sensors.SpecificData.EosSet.tf_EosHorAngRange;
            tf_3DLRFHorAngularResolution    = Sensors.SpecificData.EosSet.tf_EosHorResolution;
            tf_3DLRFVerAngularRange         = Sensors.SpecificData.EosSet.tf_EosVerAngRange;
            tf_3DLRFVerAngularResolution    = Sensors.SpecificData.EosSet.tf_EosVerResolution;
            f_3DLRFRange                    = Sensors.SpecificData.EosSet.f_Range;

            f_alphaMin = tf_3DLRFHorAngularRange[0]*(float)(Math.PI/180);
            f_sigmaMax = tf_3DLRFVerAngularRange[1] *(float)(Math.PI / 180);
         
            CellList = new ArrayList[HighResMap.us_Nrows,HighResMap.us_Ncols];
            /*
            for (int i = 0; i < HighResMap.us_Nrows; i++)
                for (int j = 0; j < HighResMap.us_Ncols; j++)
                    CellList[i, j] = new ArrayList(); 
            */
            //Calculation of the rounding error to create HRM whitout this error to compare in SLAM
          
            tf_RobotPosOrigin = M44_BasicClass.Cell2Coordinates(LowResMapGlobal, ti_RobotCell);
            f_RobotPosOriginHeight = LowResMapGlobal.MapCell[ti_RobotCell[0], ti_RobotCell[1]].f_Height;

            td_RoundingError[0] = r_RobotStateEstimate.RobotEstimate.td_Pos[0] + td_epsilon[0] - tf_RobotPosOrigin[0];
            td_RoundingError[1] = r_RobotStateEstimate.RobotEstimate.td_Pos[1] + td_epsilon[1] - f_RobotPosOriginHeight;
            td_RoundingError[2] = r_RobotStateEstimate.RobotEstimate.td_Pos[2] + td_epsilon[2] - tf_RobotPosOrigin[1];
            



            for (int i = 0; i < LaserMeasure.s_i; i++)
            {
                for (int j = 0; j < LaserMeasure.s_n; j++)
                {
                    
                    if (Lrf3DMeasure[i,j] != 0 && Lrf3DMeasure[i,j] < (Sensors.SpecificData.EosSet.f_Range - 0.1))
                    {
                        f_Distance = Lrf3DMeasure[i,j];
                        /*
                        td_aux_Cartesian = M44_BasicClass.Spherical2Cartesian(f_Distance,
                                                                 f_sigmaMax - (float)i * tf_3DLRFVerAngularResolution[0],
                                                                 f_alphaMin + (float)j * tf_3DLRFHorAngularResolution[0]);
                        tf_CartesianLocalCoord[0] = (float)td_aux_Cartesian[0,0];
                        tf_CartesianLocalCoord[1] = (float)td_aux_Cartesian[1,0];
                        tf_CartesianLocalCoord[2] = (float)td_aux_Cartesian[2,0];
                        /*
                        tf_CollisionPoint = LocalCoordRobotReference_Slam(tf_CartesianLocalCoord,
                                                                        Sensors,
                                                                        r_RobotStateEstimate,
                                                                        Orientation,
                                                                        td_RoundingError);
                         
                       
                        td_aux[0] = (double)tf_CollisionPoint[0];
                        td_aux[1] = (double)tf_CollisionPoint[1];
                        td_aux[2] = (double)tf_CollisionPoint[2];
                        */
                        tf_CartesianLocalCoord = M44_BasicClass.Spherical2Cartesian(f_Distance,
                                                                 f_sigmaMax - (float)i * tf_3DLRFVerAngularResolution[0],
                                                                 f_alphaMin + (float)j * tf_3DLRFHorAngularResolution[0]);
                        tf_CollisionPoint = M44_BasicClass.Local2FixedCoord(tf_CartesianLocalCoord,r_RobotStateEstimate,Sensors,Orientation); 
                        cl_Pos3D CollisionPoint3D = new cl_Pos3D();
                        CollisionPoint3D.f_x = (float)Math.Round(tf_CollisionPoint[0], 3);
                        CollisionPoint3D.f_y = (float)Math.Round(tf_CollisionPoint[1], 3);
                        CollisionPoint3D.f_z = (float)Math.Round(tf_CollisionPoint[2], 3);
                        
                        //correspondent map cell for the collision point
                        ti_AssignedCell = M44_BasicClass.Coordinates2Cell_High(HighResMap, tf_CollisionPoint); 
                        /*
                        if (CellList[ti_AssignedCell[0],ti_AssignedCell[1]].Count == 0)
                        {
                            CellList[ti_AssignedCell[0],ti_AssignedCell[1]] = new ArrayList();
                            CellList[ti_AssignedCell[0],ti_AssignedCell[1]].Add(tf_CollisionPoint);
                        }
                        else
                        {
                            CellList[ti_AssignedCell[0],ti_AssignedCell[1]].Add(tf_CollisionPoint);
                        }
                         */
                        /*If the cell indexes are outside the map, then break*/
                                if ((ti_AssignedCell[0] < 0) || (ti_AssignedCell[0] > (HighResMap.us_Nrows - 1)) || (ti_AssignedCell[1] < 0) || (ti_AssignedCell[1] > (HighResMap.us_Ncols - 1)))
                                {
                                    break;
                                }

                                index_i = ti_AssignedCell[0];
                                index_j = ti_AssignedCell[1];

                                /*Store colision point in its respective cell*/
                                if (CellList[index_i, index_j] == null)
                                {
                                    CellList[index_i, index_j] = new ArrayList();
                                    CellList[index_i, index_j].Add(CollisionPoint3D);
                                    colcounter = colcounter + 1;
                                }
                                else
                                {
                                    cl_Pos3D pos = new cl_Pos3D();
                                    pos = (cl_Pos3D)CellList[index_i, index_j][(CellList[index_i, index_j].Count - 1)];
                                    if (pos != CollisionPoint3D)
                                    {
                                        CellList[index_i, index_j].Add(CollisionPoint3D);
                                        colcounter = colcounter + 1;
                                    }
                                }
                                aux_MapCell_local_Slam.Distance[index_i, index_j] = LaserMeasure.tf_3DLrfDistance[i, j];
                    }
                }
            }
            
       //     aux_MapCell_local_Slam = HeightGradientEstimation_slam(CellList, aux_MapCell_local_Slam,HighResMap.us_Nrows,HighResMap.us_Ncols);
            aux_MapCell_local_Slam = M44_HeightGradientEst.Height_Gradient_Estimation(CellList, aux_MapCell_local_Slam);
            HRM_results.RoundingError[0] = (float)td_RoundingError[0];
            HRM_results.RoundingError[1] = (float)td_RoundingError[1];
            HRM_results.RoundingError[2] = (float)td_RoundingError[2];
            aux_MapCell_local_Slam2.Measured_Heights = aux_MapCell_local_Slam.Measured_Heights;
            aux_MapCell_local_Slam2.Measured_Gradient_X = aux_MapCell_local_Slam.Measured_Gradient_X;
            aux_MapCell_local_Slam2.Measured_Gradient_Y = aux_MapCell_local_Slam.Measured_Gradient_Y;
            HRM_results.aux_MapCell_local_slam = aux_MapCell_local_Slam2;
            debug_aux1 = 0;

            for(int k1 = 0;k1<HighResMap.us_Nrows;k1++)
                for (int k2 = 0; k2 < HighResMap.us_Ncols; k2++)
                {
                    debug_aux1 = Math.Max(debug_aux1, aux_MapCell_local_Slam.Measured_Heights[k1, k2]);
                    
                }

            

            return HRM_results;




        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public static M44_auxSlamStr.cl_auxMapCell HeightGradientEstimation_slam(ArrayList[,] CellList,
                                                                               M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam,
                                                                               int s_Nm1, int s_Nm2)
         * 
         *  \brief This function updates the gradient estimation in the auxMapCell
         *  
         *  \param[in] ArrayList[,] CellList,
         *  \param[in] M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam,
         *  \param[in] int s_Nm1,
         *  \param[in] int s_Nm2,
         *  
         *  \param[out] M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam 
         */
        /* ------------------------------------------------------------------------- */


        public static M44_auxSlamStr.cl_auxMapCell HeightGradientEstimation_slam(ArrayList[,] CellList,
                                                                               M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam,
                                                                               int s_Nm1, int s_Nm2)
        {
            float[] tf_actPoint = new float[3];
            float f_meanH = 0;
            int[,] celd = new int[4, 2];
            ArrayList aux_ArrayList = new ArrayList();
            Array aux_Array;
       //     int TimesOccupied = 0;
       //     int i_celd, j_celd;
            Matrix M_aux= new Matrix(2, 1);
            Matrix M_aux2 = new Matrix(1, 1);

            /*We check which cells have been explored by the sensors*/
            for (int i = 0; i < s_Nm1; i++)
            {
                for (int j = 0; j < s_Nm2; j++)
                {
                    if (CellList[i, j] != null)
                    {
                        aux_ArrayList = CellList[i, j];
                        aux_Array = aux_ArrayList.ToArray();
                        if (CellList[i, j].Count > 0)
                        {
                            f_meanH = 0;
                            for (int k = 0; k < CellList[i, j].Count; k++)
                            {
                                tf_actPoint = (float[])aux_Array.GetValue(k);
                                f_meanH = f_meanH + tf_actPoint[1];
                            }

                            aux_MapCell_local_Slam.Measured_Heights[i, j] = f_meanH / CellList[i, j].Count;
                        }
                        else
                            aux_MapCell_local_Slam.Measured_Heights[i, j] = 0;

                        if (CellList[i, j].Count > 2)
                        {
                            aux_MapCell_local_Slam.Confidence[i, j] = 1;
                            int n = CellList[i, j].Count;
                            Matrix ones = Matrix.Ones(n, 1);
                            Matrix f_x = new Matrix(n, 1);
                            Matrix f_y = new Matrix(n, 1);
                            Matrix f_z = new Matrix(n, 1);
                            aux_ArrayList = CellList[i, j];
                            aux_Array = aux_ArrayList.ToArray();
                            for (int k = 0; k < n; k++)
                            {
                                tf_actPoint = (float[])aux_Array.GetValue(k);
                                f_x[k + 1, 1].Re = tf_actPoint[0];
                                f_y[k + 1, 1].Re = tf_actPoint[2];
                                f_z[k + 1, 1].Re = tf_actPoint[1];

                            }

                            Matrix M_temp = Matrix.VerticalConcat(ones, f_x);
                            Matrix M = Matrix.VerticalConcat(M_temp, f_y);

                            Matrix tmp_M = M.Transpose() * M;

                            Complex Determinant;
                            Determinant = tmp_M.Determinant();

                            if (Determinant == Complex.Zero)
                            {
                                break;
                            }

                            Matrix P_M = tmp_M.Inverse() * M.Transpose();
                            Matrix Beta = P_M * f_z;

                            aux_MapCell_local_Slam.Measured_Gradient_X[i, j] = (float)Beta[2, 1].Re;
                            aux_MapCell_local_Slam.Measured_Gradient_Y[i, j] = (float)Beta[3, 1].Re;

                        }
                        else
                        {
                            aux_MapCell_local_Slam.Measured_Gradient_X[i, j] = 0;
                            aux_MapCell_local_Slam.Measured_Gradient_Y[i, j] = 0;
                        }
                    }
                }
            }
         /*
            for (int k1 = 0; k1 < s_Nm1 / 2; k1++)
            {
                for (int k2 = 0; k2 < s_Nm2 / 2; k2++)
                {
                    celd[0, 0] = (k1 * 2);
                    celd[0, 1] = (k2 * 2);
                    celd[1, 0] = (k1 * 2)+1;
                    celd[1, 1] = (k2 * 2);
                    celd[2, 0] = (k1 * 2);
                    celd[2, 1] = (k2 * 2)+1;
                    celd[3, 0] = (k1 * 2)+1;
                    celd[3, 1] = (k2 * 2)+1;
                    TimesOccupied = 0;
                    Matrix Celd_List_x4 = new Matrix("0;0");
                    Matrix f_z = new Matrix(1,1);
                    for (int k_celd = 0; k_celd < 4; k_celd++)
                    {
                        i_celd = celd[k_celd, 0];
                        j_celd = celd[k_celd, 1];
                        
                        TimesOccupied = TimesOccupied + CellList[i_celd, j_celd].Count;
                        aux_ArrayList = CellList[i_celd, j_celd];
                        aux_Array = aux_ArrayList.ToArray();
                        for (int k_aux = 0; k_aux < CellList[i_celd, j_celd].Count; k_aux++)
                        {
                            tf_actPoint = (float[])aux_Array.GetValue(k_aux);
                            M_aux[1, 1].Re = tf_actPoint[0];
                            M_aux[2, 1].Re = tf_actPoint[1];
                            M_aux2[1,1].Re = tf_actPoint[2];
                            Celd_List_x4 = Matrix.VerticalConcat(Celd_List_x4, M_aux);
                            f_z = Matrix.HorizontalConcat(f_z, M_aux2);
                        }

                    }
                    if (TimesOccupied > 2)
                    {
                        Celd_List_x4 = Celd_List_x4.Transpose();
                        int n = (int)Celd_List_x4.RowCount;
                        Matrix ones = Matrix.Ones(n, 1);
                        Matrix M = Matrix.VerticalConcat(ones, Celd_List_x4);

                        Matrix tmp_M = M.Transpose() * M;

                        Complex Determinant;
                        Determinant = tmp_M.Determinant();

                        if (Determinant == Complex.Zero)
                        {
                            break;
                        }
                        
                        Matrix P_M = tmp_M.Inverse() * M.Transpose();
                        Matrix Beta = P_M * f_z;

                        for (int k_celd = 0; k_celd < 4; k_celd++)
                        {
                            i_celd = celd[k_celd, 0];
                            j_celd = celd[k_celd, 1];
                            if (aux_MapCell_local_Slam.Measured_Gradient_X[i_celd, j_celd] == 0)
                            {
                                aux_MapCell_local_Slam.Measured_Gradient_X[i_celd, j_celd] = (float)Beta[2, 1].Re;
                                aux_MapCell_local_Slam.Measured_Gradient_Y[i_celd, j_celd] = (float)Beta[3, 1].Re;
                            }
                        }
                    }
                }
            }
            */
            
            return aux_MapCell_local_Slam;

        }
        

        /* ------------------------------------------------------------------------- */
        /**	\fn public static int[] Slam_Map_CellAssignment(cl_Map2D Map, float[] tf_LocalCoord)
         * 
         *  \brief This function assigns in the map the cell correspondent to the coordinates given
         *  
         *  \param[in] cl_Map2D Map,
         *  \param[in] loat[] tf_LocalCoord,
         *  
         *  \param[out] int[] AssignedCell
         */
        /* ------------------------------------------------------------------------- */
        /*
        public static int[] Slam_Map_CellAssignment(cl_Map2D Map, float[] tf_LocalCoord)
        
        {
            int s_Nm1 = Map.us_Nrows;
            int s_Nm2 = Map.us_Ncols;
            int s_Scale = Map.us_Scale;
            int[] AssignedCell = new int[2];
            double[] d_Cm = new double[2];
            int i,j;
            d_Cm[0] = (s_Nm2 / 2)-1;
            d_Cm[1] = s_Nm1 / 2;

            if (tf_LocalCoord[0] >= 0)
                j = (int)(Math.Truncate((double)(tf_LocalCoord[0] / s_Scale)-1) + d_Cm[0]);
            else
                j = (int)(Math.Truncate((double)tf_LocalCoord[0] / s_Scale) + d_Cm[0]);
            if (tf_LocalCoord[1] >= 0)
                i = (int)(d_Cm[0] - Math.Truncate((double)(tf_LocalCoord[1] / s_Scale) - 1));
            else
                i = (int)(d_Cm[0] - Math.Truncate((double)(tf_LocalCoord[1] / s_Scale) - 2));

            AssignedCell[0] = i;
            AssignedCell[1] = j;

            return AssignedCell;
        }

        */

        /* ------------------------------------------------------------------------- */
        /**	\fn ppublic static float[] LocalCoordRobotReference_Slam(float[] tf_CartesianLocalCoord,
                                                cl_SensorData Sensors,
                                                cl_RobotStateEstimate r_RobotStateEstimate,
                                                double[] td_RoundingError)
         * 
         *  \brief Given the cartesian coordinates and the rounding error this function returns the collision point
         *  
         *  \param[in] float[] tf_CartesianLocalCoord
         *  \param[in] cl_SensorData Sensors,
         *  \param[in] cl_RobotStateEstimate r_RobotStateEstimate,
         *  \param[in] double[] td_RoundingError,
         *  
         *  \param[out] float[] tf_CollisionPoint
         */
        /* ------------------------------------------------------------------------- */
        public static float[] LocalCoordRobotReference_Slam(float[] tf_CartesianLocalCoord,
                                                cl_SensorData Sensors,
                                                cl_RobotGPSINSState r_RobotStateEstimate,
                                                cl_Orientation3D Orientation,
                                                double[] td_RoundingError)
        {
            float[] tf_3DLRFPosition = new float[3];
            double[] RobotPos = new double[3];
            double phi, theta, psi;
            xna.Matrix Rot12 = new xna.Matrix();
            xna.Matrix T12 = new xna.Matrix();
            xna.Matrix T23 = new xna.Matrix();
            xna.Matrix CartesianLocalMatrix = new xna.Matrix();
            xna.Matrix ResultMatrix = new xna.Matrix();
            float[] tf_CollisionPoint = new float[3];


            RobotPos[0] = r_RobotStateEstimate.RobotEstimate.td_Pos[0];
            RobotPos[1] = r_RobotStateEstimate.RobotEstimate.td_Pos[1];
            RobotPos[2] = r_RobotStateEstimate.RobotEstimate.td_Pos[2];

            tf_3DLRFPosition[0] = (float)Sensors.Position.d_x;
            tf_3DLRFPosition[1] = (float)Sensors.Position.d_y;
            tf_3DLRFPosition[2] = (float)Sensors.Position.d_z;

            phi = r_RobotStateEstimate.RobotEstimate.td_Attitude[0] + Orientation.d_phi;
            theta = r_RobotStateEstimate.RobotEstimate.td_Attitude[1] + Orientation.d_chi;
            psi = r_RobotStateEstimate.RobotEstimate.td_Attitude[2] + Orientation.d_psi;
            
            /*Rot12 Matrix*/
            /*First column*/
            Rot12.M11 = (float)(Math.Cos(psi) * Math.Cos(theta));
            Rot12.M21 = (float)(Math.Sin(psi) * Math.Cos(theta));
            Rot12.M31 = (float)(-Math.Sin(theta));
            Rot12.M41 = 0;

            /*Second column*/
            Rot12.M12 = (float)(Math.Cos(psi) * Math.Sin(theta) * Math.Sin(phi) - Math.Sin(psi) * Math.Cos(phi));
            Rot12.M22 = (float)(Math.Sin(psi) * Math.Sin(theta) * Math.Sin(phi) + Math.Cos(psi) * Math.Cos(phi));
            Rot12.M32 = (float)(Math.Cos(theta) * Math.Sin(phi));
            Rot12.M42 = 0;

            /*Third column*/
            Rot12.M13 = (float)(Math.Cos(psi) * Math.Sin(theta) * Math.Cos(phi) + Math.Sin(psi) * Math.Sin(phi));
            Rot12.M23 = (float)(Math.Sin(psi) * Math.Sin(theta) * Math.Cos(phi) - Math.Cos(psi) * Math.Sin(phi));
            Rot12.M33 = (float)(Math.Cos(theta) * Math.Cos(phi));
            Rot12.M43 = 0;

            /*Fourth column*/
            Rot12.M14 = 0;
            Rot12.M24 = 0;
            Rot12.M34 = 0;
            Rot12.M44 = 1;

            /*T12 Matrix*/
            /*First column*/
            T12.M11 = 1;
            T12.M21 = 0;
            T12.M31 = 0;
            T12.M41 = 0;

            /*Second column*/
            T12.M12 = 0;
            T12.M22 = 1;
            T12.M32 = 0;
            T12.M42 = 0;

            /*Third column*/
            T12.M13 = 0;
            T12.M23 = 0;
            T12.M33 = 1;
            T12.M43 = 0;

            /*Fourth column*/
            T12.M14 = (float)RobotPos[0];// td_RoundingError[0];
            T12.M24 = (float)RobotPos[1];// td_RoundingError[2];
            T12.M34 = (float)RobotPos[2];// td_RoundingError[1];
            T12.M44 = 1;

            /*T23 Matrix*/
            /*First column*/
            T23.M11 = 1;
            T23.M21 = 0;
            T23.M31 = 0;
            T23.M41 = 0;

            /*Second column*/
            T23.M12 = 0;
            T23.M22 = 1;
            T23.M32 = 0;
            T23.M42 = 0;

            /*Third column*/
            T23.M13 = 0;
            T23.M23 = 0;
            T23.M33 = 1;
            T23.M43 = 0;

            /*Fourth column*/
            T23.M14 = tf_3DLRFPosition[0];
            T23.M24 = tf_3DLRFPosition[1];
            T23.M34 = tf_3DLRFPosition[2];
            T23.M44 = 1;

            /*CartesianLocal Matrix*/
            /*First column*/
            CartesianLocalMatrix.M11 = tf_CartesianLocalCoord[0];
            CartesianLocalMatrix.M21 = tf_CartesianLocalCoord[1];
            CartesianLocalMatrix.M31 = tf_CartesianLocalCoord[2];
            CartesianLocalMatrix.M41 = 1;

            /*Second column*/
            CartesianLocalMatrix.M12 = 0;
            CartesianLocalMatrix.M22 = tf_CartesianLocalCoord[1];
            CartesianLocalMatrix.M32 = 0;
            CartesianLocalMatrix.M42 = 0;

            /*Third column*/
            CartesianLocalMatrix.M13 = 0;
            CartesianLocalMatrix.M23 = 0;
            CartesianLocalMatrix.M33 = tf_CartesianLocalCoord[2];
            CartesianLocalMatrix.M43 = 0;

            /*Fourth column*/
            CartesianLocalMatrix.M14 = 0.0f;
            CartesianLocalMatrix.M24 = 0.0f;
            CartesianLocalMatrix.M34 = 0.0f;
            CartesianLocalMatrix.M44 = 1;

            ResultMatrix = xna.Matrix.Multiply(T12, xna.Matrix.Multiply(Rot12, xna.Matrix.Multiply(T23, CartesianLocalMatrix)));
            tf_CollisionPoint[0] = ResultMatrix.M11;
            tf_CollisionPoint[1] = ResultMatrix.M21;
            tf_CollisionPoint[2] = ResultMatrix.M31;

            return tf_CollisionPoint;

            

        }


        /* ------------------------------------------------------------------------- */
        /**	\fn M44_auxSlamStr.cl_NewGrids NewEGridCalculation(double[,] eGrid,
         *                                                    double[,,] PosGrid,
         *                                                     int[] minIndex)
         * 
         *  \brief This function gives the new grids for the subcell error calculations.
         *  
         *  \param[in] double[,] eGrid
         *  \param[in] double[,,] PosGrid
         *  \param[in] int[] minIndex
         *  
         *  \param[out] M44_auxSlamStr.cl_NewGrids r_NewGrids
         */
        /* ------------------------------------------------------------------------- */
        public static M44_auxSlamStr.cl_NewGrids NewEGridCalculation(double[,] eGrid,double[,,] PosGrid,int[] minIndex)
        {
            double[,] eGrid2 = { { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } };
            double[, ,] PosGrid2 = { { { 0, 0 }, { 0, 0 }, { 0, 0 } }, { { 0, 0 }, { 0, 0 }, { 0, 0 } }, { { 0, 0 }, { 0, 0 }, { 0, 0 } } };
            M44_auxSlamStr.cl_NewGrids r_NewGrids = new M44_auxSlamStr.cl_NewGrids();

            eGrid2[1,1] = eGrid[minIndex[0],minIndex[1]];
            PosGrid2[1, 1, 0] = PosGrid[minIndex[0], minIndex[1], 0];
            PosGrid2[1, 1, 1] = PosGrid[minIndex[0], minIndex[1], 1];

            if (minIndex[0] == 1)
            {
                eGrid2[0,1] = eGrid[0,1];
                eGrid2[1,1] = eGrid[1,1];
                eGrid2[2,1] = eGrid[2,1];
                PosGrid2[0,1,0] = PosGrid[0,1,0]; PosGrid2[0,1,1] = PosGrid[0,1,1];
                PosGrid2[1,1,0] = PosGrid[1,1,0]; PosGrid2[1,1,1] = PosGrid[1,1,1];
                PosGrid2[2,1,0] = PosGrid[2,1,0]; PosGrid2[2,1,1] = PosGrid[2,1,1];

                if (minIndex[1] == 0)
                {
                    eGrid2[0,2] = eGrid[0,2];
                    eGrid2[1,2] = eGrid[1,2];
                    eGrid2[2,2] = eGrid[2,2];
                    PosGrid2[0,2,0] = PosGrid[0,2,0]; PosGrid2[0,2,1] = PosGrid[0,2,1];
                    PosGrid2[1,2,0] = PosGrid[1,2,0]; PosGrid2[1,2,1] = PosGrid[1,2,1];
                    PosGrid2[2,2,0] = PosGrid[2,2,0]; PosGrid2[2,2,1] = PosGrid[2,2,1];
                }
                if (minIndex[1] == 2)
                {
                    eGrid2[0,0] = eGrid[0,0];
                    eGrid2[1,0] = eGrid[1,0];
                    eGrid2[2,0] = eGrid[2,0];
                    PosGrid2[0,0,0] = PosGrid[0,0,0]; PosGrid2[0,2,1] = PosGrid[0,0,1];
                    PosGrid2[1,0,0] = PosGrid[1,0,0]; PosGrid2[1,2,1] = PosGrid[1,0,1];
                    PosGrid2[2,0,0] = PosGrid[2,0,0]; PosGrid2[2,2,1] = PosGrid[2,0,1];
                }
            }
            if (minIndex[1] == 1)
            {
                if (minIndex[0] == 0)
                {
                    eGrid2[1,0] = eGrid[0,0];
                    eGrid2[1,1] = eGrid[0,1];
                    eGrid2[1,2] = eGrid[0,2];
                    PosGrid2[1,0,0] = PosGrid[0,0,0]; PosGrid2[1,0,1] = PosGrid[0,0,1];
                    PosGrid2[1,1,0] = PosGrid[0,1,0]; PosGrid2[1,1,1] = PosGrid[0,1,1];
                    PosGrid2[1,2,0] = PosGrid[0,2,0]; PosGrid2[1,2,1] = PosGrid[0,2,1];
                    eGrid2[2,0] = eGrid[1,0];
                    eGrid2[2,1] = eGrid[1,1];
                    eGrid2[2,2] = eGrid[1,2];
                    PosGrid2[2,0,0] = PosGrid[1,0,0]; PosGrid2[2,0,1] = PosGrid[1,0,1];
                    PosGrid2[2,1,0] = PosGrid[1,1,0]; PosGrid2[2,1,1] = PosGrid[1,1,1];
                    PosGrid2[2,2,0] = PosGrid[1,2,0]; PosGrid2[2,2,1] = PosGrid[1,2,1];
                }
                if (minIndex[0] == 2)
                {
                    eGrid2[0,0] = eGrid[1,0];
                    eGrid2[0,1] = eGrid[1,1];
                    eGrid2[0,2] = eGrid[1,2];
                    PosGrid2[0,0,0] = PosGrid[1,0,0]; PosGrid2[0,0,1] = PosGrid[1,0,1];
                    PosGrid2[0,1,0] = PosGrid[1,1,0]; PosGrid2[0,1,1] = PosGrid[1,1,1];
                    PosGrid2[0,2,0] = PosGrid[1,2,0]; PosGrid2[0,2,1] = PosGrid[1,2,1];
                    eGrid2[1,0] = eGrid[2,0];
                    eGrid2[1,1] = eGrid[2,1];
                    eGrid2[1,2] = eGrid[2,2];
                    PosGrid2[1,0,0] = PosGrid[2,0,0]; PosGrid2[1,0,1] = PosGrid[2,0,1];
                    PosGrid2[1,1,0] = PosGrid[2,1,0]; PosGrid2[1,1,1] = PosGrid[2,1,1];
                    PosGrid2[1,2,0] = PosGrid[2,2,0]; PosGrid2[1,2,1] = PosGrid[2,2,1];
                }
            }
            if (minIndex[0] == 0)
            {
                if (minIndex[1] == 0)
                {
                    eGrid2[1,1] = eGrid[0,0];
                    eGrid2[1,2] = eGrid[0,1];
                    eGrid2[2,1] = eGrid[1,0];
                    eGrid2[2,2] = eGrid[1,1];
                    PosGrid2[1,1,0] = PosGrid[0,0,0]; PosGrid2[1,1,1] = PosGrid[0,0,1];
                    PosGrid2[1,2,0] = PosGrid[0,1,0]; PosGrid2[1,2,1] = PosGrid[0,1,1];
                    PosGrid2[2,1,0] = PosGrid[1,0,0]; PosGrid2[2,1,1] = PosGrid[1,0,1];
                    PosGrid2[2,2,0] = PosGrid[1,1,0]; PosGrid2[2,2,1] = PosGrid[1,1,1];
                }
                if (minIndex[1] == 2)
                {
                    eGrid2[1,0] = eGrid[0,1];
                    eGrid2[1,1] = eGrid[0,2];
                    eGrid2[2,0] = eGrid[1,1];
                    eGrid2[2,2] = eGrid[1,2];
                    PosGrid2[1,0,0] = PosGrid[0,1,0]; PosGrid2[1,0,1] = PosGrid[0,1,1];
                    PosGrid2[1,1,0] = PosGrid[0,2,0]; PosGrid2[1,1,1] = PosGrid[0,2,1];
                    PosGrid2[2,0,0] = PosGrid[1,1,0]; PosGrid2[2,0,1] = PosGrid[1,1,1];
                    PosGrid2[2,1,0] = PosGrid[1,2,0]; PosGrid2[2,1,1] = PosGrid[1,2,1];
                }
            }
            if (minIndex[0] == 2)
            {
                if (minIndex[1] == 0)
                {
                    eGrid2[0,1] = eGrid[1,0];
                    eGrid2[0,2] = eGrid[1,1];
                    eGrid2[1,1] = eGrid[2,0];
                    eGrid2[1,2] = eGrid[2,1];
                    PosGrid2[0,1,0] = PosGrid[1,0,0]; PosGrid2[0,1,1] = PosGrid[1,0,1];
                    PosGrid2[0,2,0] = PosGrid[1,1,0]; PosGrid2[0,2,1] = PosGrid[1,1,1];
                    PosGrid2[1,1,0] = PosGrid[2,0,0]; PosGrid2[1,1,1] = PosGrid[2,0,1];
                    PosGrid2[1,2,0] = PosGrid[2,1,0]; PosGrid2[1,2,1] = PosGrid[2,1,1];
                }
                if (minIndex[1] == 2)
                {
                    eGrid2[0,0] = eGrid[1,1];
                    eGrid2[0,1] = eGrid[1,2];
                    eGrid2[1,0] = eGrid[2,1];
                    eGrid2[1,1] = eGrid[2,2];
                    PosGrid2[0,0,0] = PosGrid[1,1,0]; PosGrid2[0,0,1] = PosGrid[1,1,1];
                    PosGrid2[0,1,0] = PosGrid[1,2,0]; PosGrid2[0,1,1] = PosGrid[1,2,1];
                    PosGrid2[1,0,0] = PosGrid[2,1,0]; PosGrid2[1,0,1] = PosGrid[2,1,1];
                    PosGrid2[1,1,0] = PosGrid[2,2,0]; PosGrid2[1,1,1] = PosGrid[2,2,1];
                }
            }

            r_NewGrids.eGrid = eGrid2;
            r_NewGrids.PosGrid = PosGrid2;

            return r_NewGrids;
        }
    
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
